﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Mogre;

namespace WastedMechanics.NavMesh
{
    public class MarshrouteNode
    {
        public Polygon Polygon;
        public Edge Portal;

        public MarshrouteNode(Polygon poly)
        {
            Polygon = poly;
        }
    }

    public class MarschrouteOptimizer
    {
        public LinkedList<MarshrouteNode> Nodes;                

        public MarschrouteOptimizer()
        {
            Nodes = new LinkedList<MarshrouteNode>();            
        }

        public void AddPolygon(Polygon poly)
        {            
            if (Nodes.Count != 0)
            {                
                MarshrouteNode previous = Nodes.Last.Value;
                previous.Portal = poly.Edges.Find(edge => previous.Polygon.Edges.Contains(edge));
            }
            Nodes.AddLast(new MarshrouteNode(poly));
        }

        public void FindLeftRight(Edge edge, Polygon poly, out Vector2 left, out Vector2 right)
        {
            Vector2 leftDir = Helper.ProjXZ(edge.Begin.Position - poly.Center).NormalisedCopy;
            Vector2 rightDir = Helper.ProjXZ(edge.End.Position - poly.Center).NormalisedCopy;
            if (leftDir.CrossProduct(rightDir) > 0) // is ok
            {
                left = Helper.ProjXZ(edge.Begin.Position);
                right = Helper.ProjXZ(edge.End.Position);
            }
            else // invert order
            {
                left = Helper.ProjXZ(edge.End.Position);
                right = Helper.ProjXZ(edge.Begin.Position);
            }
        }

        public void CalculateDirs(Vector2 leftPt, Vector2 rightPt, Vector2 lastPt, out Vector2 leftDir, out Vector2 rightDir)
        {
            leftDir = (leftPt - lastPt).NormalisedCopy;
            rightDir = (rightPt - lastPt).NormalisedCopy;   
        }

        public void GetPoints(Vector3 startPoint, Vector3 endPoint, List<Vector3> points)
        {
            points.Clear();               
            var currentNode = Nodes.First;
            MarshrouteNode current = currentNode.Value;

            Vector2 curPoint = Helper.ProjXZ(startPoint);
            Vector2 leftPoint, rightPoint;
            Vector2 currLeftPoint, currRightPoint;
            Vector2 leftDir, rightDir;
            Vector2 currLeftDir, currRightDir;
            Vector3 polyCenter = current.Polygon.Center;            

            var nearestLeftNode = currentNode;
            var nearestRightNode = currentNode;
                                  
            if (current.Portal != null)
                FindLeftRight(current.Portal, current.Polygon, out leftPoint, out rightPoint);
            else
            {
                // startpoly == endpoly
                return;
            }

            CalculateDirs(leftPoint, rightPoint, curPoint, out leftDir, out rightDir);

            while (true)
            {
                current = currentNode.Value;
                //current.Polygon.Visualize();
                if (current.Portal != null)
                {
                    FindLeftRight(current.Portal, current.Polygon, out currLeftPoint, out currRightPoint);
                    CalculateDirs(currLeftPoint, currRightPoint, curPoint, out currLeftDir, out currRightDir);
                    //Helper.DrawLine("Debug/HardBlue", new Vector3(curPoint.x, 0, curPoint.y), new Vector3(currLeftPoint.x, 0, currLeftPoint.y));
                    //Helper.DrawLine("Debug/HardGreen", new Vector3(curPoint.x, 0, curPoint.y), new Vector3(currRightPoint.x, 0, currRightPoint.y));

                    if (currRightDir.CrossProduct(leftDir) >= 0) // all behind the left
                    {
                        currentNode = nearestLeftNode.Next;
                        current = currentNode.Value;                        
                        curPoint = leftPoint;
                        FindLeftRight(currentNode.Previous.Value.Portal, currentNode.Previous.Value.Polygon, out leftPoint, out rightPoint);
                        curPoint += (rightPoint - leftPoint).NormalisedCopy * 0.5f; // move a half menter inside edge
                        points.Add(new Vector3(curPoint.x, 0, curPoint.y));

                        // Nie wiem czemu czasami portal jest nullem...
                        if (current.Portal == null)
                            return;

                        FindLeftRight(current.Portal, current.Polygon, out leftPoint, out rightPoint);
                        CalculateDirs(leftPoint, rightPoint, curPoint, out leftDir, out rightDir);
                        nearestLeftNode = currentNode;
                        nearestRightNode = currentNode;                        
                    }
                    else if (currLeftDir.CrossProduct(rightDir) <= 0) // all behind the right
                    {                        
                        currentNode = nearestRightNode.Next;
                        current = currentNode.Value;
                        curPoint = rightPoint;
                        FindLeftRight(currentNode.Previous.Value.Portal, currentNode.Previous.Value.Polygon, out leftPoint, out rightPoint);
                        curPoint += (leftPoint - rightPoint).NormalisedCopy * 0.5f;     // move a half menter inside edge            
                        points.Add(new Vector3(curPoint.x, 0, curPoint.y));

                        FindLeftRight(current.Portal, current.Polygon, out leftPoint, out rightPoint);
                        CalculateDirs(leftPoint, rightPoint, curPoint, out leftDir, out rightDir);
                        nearestLeftNode = currentNode;
                        nearestRightNode = currentNode;
                    }
                    else
                    {
                        if (currLeftDir.CrossProduct(leftDir) <= 0)
                        {
                            leftPoint = currLeftPoint;
                            leftDir = currLeftDir;
                            nearestLeftNode = currentNode;
                        }
                        if (currRightDir.CrossProduct(rightDir) >= 0)
                        {
                            rightPoint = currRightPoint;
                            rightDir = currRightDir;
                            nearestRightNode = currentNode;
                        }                        
                    }                    
                }
                else
                {
                    Vector2 goalDir = (Helper.ProjXZ(endPoint) - curPoint).NormalisedCopy;
                    if (leftDir.CrossProduct(goalDir) < 0)
                    {
                        currentNode = nearestLeftNode.Next;
                        current = currentNode.Value;                        
                        curPoint = leftPoint;
                        FindLeftRight(currentNode.Previous.Value.Portal, currentNode.Previous.Value.Polygon, out leftPoint, out rightPoint);
                        curPoint += (rightPoint - leftPoint).NormalisedCopy * 0.5f; // move a half menter inside edge
                        points.Add(new Vector3(curPoint.x, 0, curPoint.y));                        
                        if (currentNode == Nodes.Last)
                            break;

                        FindLeftRight(current.Portal, current.Polygon, out leftPoint, out rightPoint);
                        CalculateDirs(leftPoint, rightPoint, curPoint, out leftDir, out rightDir);                        
                    }
                    else if (rightDir.CrossProduct(goalDir) > 0)
                    {
                        currentNode = nearestRightNode.Next;
                        current = currentNode.Value;                        
                        curPoint = rightPoint;
                        FindLeftRight(currentNode.Previous.Value.Portal, currentNode.Previous.Value.Polygon, out leftPoint, out rightPoint);
                        curPoint += (leftPoint - rightPoint).NormalisedCopy * 0.5f; // move a half menter inside edge
                        points.Add(new Vector3(curPoint.x, 0, curPoint.y));                        
                        if (currentNode == Nodes.Last)
                            break;

                        FindLeftRight(current.Portal, current.Polygon, out leftPoint, out rightPoint);
                        CalculateDirs(leftPoint, rightPoint, curPoint, out leftDir, out rightDir);                                  
                    }
                    else
                        break;
                }
                currentNode = currentNode.Next;   
            }                    

        }

    }
}
